#include "zcurve.h"
#include <sstream>

template <size_t N>
int8_t ZPoint<N>::compare(const ZPoint<N>& that) const {
    int8_t val = 0;
    int8_t i = N;
    while (val == 0 && --i >= 0){
        val = this->test(i) - that.test(i);
    }
    return val;
}

template <size_t N>
ZPoint<N>& ZPoint<N>::operator++(){
    bool set = false;
    uint8_t i = 0;

    while (! set && i < N){
        set = ! this->test(i);
        this->flip(i++);
    }

    return *this;
}

template <size_t N>
void ZPoint<N>::read(BinaryStream& in){
    this->reset();
    uint64_t part=0, mask=1;
    for(uint8_t i = 0; i < N; i++){
        if (i % 8 == 0) {
            part = in.readPrimitive<uint8_t>();
            mask = 1;
        }
        this->set(i, part & mask);
        mask <<= 1;
    }
}

template <size_t N>
void ZPoint<N>::write(BinaryStream& out){
    uint64_t part=0, mask=1;
    uint8_t i;
    for(i = 0; i < N; i++){
        if (i && i % 8 == 0) {
            out.writePrimitive<uint8_t>(part);
            mask = 1;
            part = 0;
        }

        if (this->test(i))
           part |= mask;
        mask <<= 1;
    }

    out.writePrimitive<uint8_t>(part);
}

template <size_t N>
std::string ZPoint<N>::toString(){
    uint64_t part=0, mask=1;
    uint8_t i;
    float f = 0;
    for(i = 0; i < N; i++){
        if (i && i % 64 == 0) {
            f += part;
            mask = 1;
            part = 0;
        }

        if (this->test(i))
           part |= mask;
        mask <<= 1;
    }
    f += part;
    std::stringstream sout;
    sout << f;
    return sout.str();
}

template <size_t D, size_t K>
ZPoint<D*K> ZCurve<D, K>::map(Point<D> point){
    ZPoint<D*K> zPoint;

    for (int8_t i = D*K - 1; i >= 0; i--){ //MSB -> LSB
        uint8_t k = (D*K - i - 1) % D;
        uint8_t s = i / D;
        zPoint.set(i, point[k] & (1 << s));
    }
    return zPoint;
}

template <size_t D, size_t K>
Point<D> ZCurve<D, K>::inv(ZPoint<D*K> zPoint){
    Point<D> point = {0};
    point.fill(0);

    for (int8_t i = D*K - 1; i >= 0; i--){  //MSB -> LSB
        uint8_t k = (D*K - i - 1) % D;
        uint8_t s = i / D;
        point[k] |= zPoint.test(i) << s;
    }
    return point;
}

template <size_t D, size_t K>
bool ZCurve<D, K>::inRange(ZPoint<D*K> zMin, ZPoint<D*K> zMax, ZPoint<D*K> zPoint){

    bool aboveMin[D] = {0};
    bool belowMax[D] = {0};

    for (int8_t i = D*K - 1; i >= 0; i--) {  //MSB -> LSB
        uint8_t k = (D*K - i - 1) % D;

        if (aboveMin[k] && belowMax[k])
            continue;

        uint8_t bMin = zMin.test(i);
        uint8_t bMax = zMax.test(i);
        uint8_t bCur = zPoint.test(i);

        if (! belowMax[k]) {
            if (bMax > bCur)
                belowMax[k] = true;
            if (bCur > bMax)
                return false;
        }

        if (! aboveMin[k]) {
            if (bMin < bCur)
                aboveMin[k] = true;
            if (bMin > bCur)
                return false;
        }
    }
    return true;
}

template <size_t D, size_t K>
ZPoint<D*K> ZCurve<D, K>::nextZAddress(ZPoint<D*K> zMin, ZPoint<D*K> zMax, ZPoint<D*K> zCur){

    ZPoint<D*K> zNext = zCur;

    int8_t flags[D] = {0};
    int8_t outStep[D];
    int8_t saveMin[D];
    int8_t saveMax[D];

    for (uint8_t k = 0; k < D; k++) {
        outStep[k] = -1;
        saveMin[k] = -1;
        saveMax[k] = -1;
    }

    for (int8_t i = D*K - 1; i >= 0; i--) {  //MSB -> LSB
        uint8_t k = (D*K - i - 1) % D;

        uint8_t bMin = zMin.test(i);
        uint8_t bMax = zMax.test(i);
        uint8_t bCur = zCur.test(i);

        if (saveMax[k] < 0 && outStep[k] < 0){
            if (bMax > bCur) {
                saveMax[k] = (uint8_t) i;
            }
            if (bCur > bMax) {
                outStep[k] = (uint8_t) i;
                flags[k] = 1;
            }
        }

        if (saveMin[k] < 0 && outStep[k] < 0){
            if (bMin < bCur) {
                    saveMin[k] = (uint8_t) i;
            }
            if (bMin > bCur) {
                outStep[k] = (uint8_t) i;
                flags[k] = -1;
            }
        }
    }

    int8_t dim = -1;
    int8_t switchBit = -1;
    for (uint8_t k = 0; k < D; k++) {
        if (outStep[k] > switchBit) {
            switchBit = outStep[k];
            dim = k;
        }
    }

    if (switchBit >= 0) {

        if (flags[dim] == 1) {
            for (uint8_t i = switchBit+1; i < D*K; i++){  //LSB -> MSB
                uint8_t k = (D*K - i - 1) % D;
                if (k == dim)
                    continue;

                if (i <= saveMax[k] && (zNext.test(i) == 0)) {
                    switchBit = i;
                    dim = k;
                    break;
                }
            }
            saveMin[dim] = switchBit;
            flags[dim] = 0;
        }

        zNext.flip(switchBit--);

        for (int8_t i = D*K - 1; i >= 0; i--) {  //MSB -> LSB
            uint8_t k = (D*K - i - 1) % D;

            if (flags[k] >= 0 && i > switchBit)
                continue;

            zNext.set(i, 0);
            if (flags[k] < 0 || switchBit >= saveMin[k])
                zNext.set(i, zMin.test(i) != 0);
        }
    }
    return zNext;
}
